#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp> //opencv highgui 模块头文件
#include <opencv2/imgproc/imgproc.hpp> //opencv 图像处理头文件
#include <iostream>
#include <string>
#include <opencv2/video/tracking.hpp>

using namespace std;
using namespace cv;

void onChange(int, void *)
{
	imshow("control", 0);
}

Mat frame;

int main()
{

	bool stop = true;
	VideoCapture cap(0);

	namedWindow("control", WINDOW_FREERATIO);
	//识别颜色
	int lowB = 110;
	int highB = 255;
	int lowG = 120;
	int highG = 199;
	int lowR = 136;
	int highR = 255;

	createTrackbar("lowB", "control", &lowB, 255, onChange);
	createTrackbar("highB", "control", &highB, 255, onChange);
	createTrackbar("lowG", "control", &lowG, 255, onChange);
	createTrackbar("highG", "control", &highG, 255, onChange);
	createTrackbar("lowR", "control", &lowR, 255, onChange);
	createTrackbar("highR", "control", &highR, 255, onChange);
	onChange(0, 0);

	// 1.kf setup
	const int stateNum = 4;	  //状态值4×1向量(x,y,△x,△y)
	const int measureNum = 2; //测量值2×1向量(x,y)
	KalmanFilter KF(stateNum, measureNum, 0);

	KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); //转移矩阵A
	setIdentity(KF.measurementMatrix);															 // Scalar::all(1)                                             //测量矩阵H
	setIdentity(KF.processNoiseCov, Scalar::all(1e-5));											 //(1e-3)                        //系统噪声方差矩阵Q
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));										 //测量噪声方差矩阵R
	setIdentity(KF.errorCovPost, Scalar::all(1));												 //后验错误估计协方差矩阵P

	randn(KF.statePost, Scalar::all(0), Scalar::all(0.1)); // x(0)初始化
	Mat measurement = Mat::zeros(measureNum, 1, CV_32F);   //两行一列测量矩阵

	if (!cap.isOpened())
	{
		cout << "video is error! " << endl;
		return 0;
	}
	while (stop)
	{
		cap >> frame;
		if (!frame.empty())
		{
			imshow("frame", frame);

			Mat src = frame.clone();
			Mat imghsv;
			vector<Mat> hsvSplit;
			cvtColor(src, imghsv, COLOR_BGR2HSV);
			split(imghsv, hsvSplit);
			merge(hsvSplit, imghsv);

			// Mat imgbgr;
			// cvtColor(imghsv, imgbgr, COLOR_HSV2BGR);

			Mat imgthre;
			inRange(imghsv, Scalar(lowB, lowG, lowR), Scalar(highB, highG, highR), imgthre);
			// inRange(imgbgr, Scalar(lowB, lowG, lowR), Scalar(highB, highG, highR), imgthre);//类似多通道二值化处理
			Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
			morphologyEx(imgthre, imgthre, MORPH_OPEN, element);
			morphologyEx(imgthre, imgthre, MORPH_CLOSE, element);
			imshow("imgthre", imgthre);
			vector<vector<Point>> contours;
			findContours(imgthre, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
			vector<Rect> rect(contours.size());
			vector<double> area;
			area.clear();
			for (int i = 0; i < contours.size(); i++)
			{
				area.push_back(contourArea(contours[i]));
			}
			int maxPosition = max_element(area.begin(), area.end()) - area.begin(); //求面积最大的轮廓索引
			Rect rects = boundingRect(contours[maxPosition]);
			int x = rects.x;
			int y = rects.y;
			int width = rects.width;
			int height = rects.height;
			rectangle(frame, Point(x, y), Point(x + width, y + height), Scalar(0, 255, 0), 2);

			double x0, y0;
			x0 = x + width / 2;
			y0 = y + height / 2;
			circle(frame, Point(x0, y0), 2, Scalar(255, 0, 0), 1, 8, 0); //实际点 蓝色

			cout << "实际点坐标：" << Point(x0, y0) << endl;

			///////////////////////////////////////////////////////////////////////////////////////////////////

			// KF.statePost = (Mat_<float>(4, 1) << x0, y0, 0, 0);
			// 2.kalman prediction

			float anti_range = 0.5;       		//反卡尔曼滤波的一个参数
			Point2f currentPoint(x0, y0); 		//当前测量点
			Point2f kalmanPoint(x0, y0);  		//卡尔曼滤波预测点
			Point2f anti_kalmanPoint(x0, y0);	//反卡尔曼滤波预测点

			Mat prediction = KF.predict();
			Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1)); //预测值(x',y')
			kalmanPoint = predict_pt;

			// 3.update measurement
			measurement.at<float>(0) = (float)Point(x0, y0).x;
			measurement.at<float>(1) = (float)Point(x0, y0).y;
			// 4.update
			KF.correct(measurement);        	//融合预测量和测量量

			if ((currentPoint.x + anti_range * (currentPoint.x - kalmanPoint.x)) <= 640 || (currentPoint.x + anti_range * (currentPoint.x - kalmanPoint.x)) >= 0) // Prevent Anti-kal out of Mat
			{
				if (abs(currentPoint.x - kalmanPoint.x) > 3) // When points are closed, no Anti-kalman to reduce shaking
					anti_kalmanPoint.x = currentPoint.x + anti_range * (currentPoint.x - kalmanPoint.x);
				else
					anti_kalmanPoint.x = currentPoint.x;
			}
			else
			{
				anti_kalmanPoint.x = currentPoint.x;
			}

			if ((currentPoint.y + anti_range * (currentPoint.y - kalmanPoint.y)) <= 480 || (currentPoint.y + anti_range * (currentPoint.y - kalmanPoint.y)) >= 0) // Prevent Anti-kal out of Mat
			{
				if (abs(currentPoint.y - kalmanPoint.y) > 3) // When points are closed, no Anti-kalman to reduce shaking
					anti_kalmanPoint.y = currentPoint.y + anti_range * (currentPoint.y - kalmanPoint.y);
				else
					anti_kalmanPoint.y = currentPoint.y;
			}
			else
			{
				anti_kalmanPoint.y = currentPoint.y;
			}

			circle(frame, predict_pt, 4, Scalar(139, 139, 0), -1, 8, 0);	 //卡尔曼预测点 绿色
			circle(frame, anti_kalmanPoint, 4, Scalar(0, 0, 255), -1, 8, 0); //反卡尔曼预测点 红色

			cout << "预测点坐标：" << anti_kalmanPoint << endl;

			imshow("result", frame);
		}
		if (waitKey(30) > 0)
			stop = false;
	}
	return 0;
}

void predict2D()
{
	
}